#include "HOT.h"
#include <teem/tijk.h>

static const float factor[2][28] = { 
	{1, 4, 6, 4, 1, 4, 12, 12, 4, 6, 12, 6, 4, 4, 1},
	{1,6,15,20,15,6,1,6,30,60,60,30,6,15,60,90,60,15,20,60,60,20,15,30,15,6,6,1}
};

CHOT::CHOT(void)
{
	m_pHOT = NULL;
	m_w = m_h = m_d = 0;
	m_n = 0;
	m_order = 0;
}


CHOT::~CHOT(void)
{
	SafeDeleteArray(m_pHOT);
	m_w = m_h = m_d = 0;
	m_n = 0;
	m_order = 0;
}


int CHOT::OpenHOTFile(const char *pathname, FILE *fp)
{
	if (pathname == NULL)
		return -1;				// no input pathname

	fprintf(fp, "Reading HOT file %s ... ", pathname);

	Nrrd *hot = nrrdNew();
	if (nrrdLoad(hot, pathname, NULL)) {
		fprintf(fp, "cannot load file %s:\n%s\n", pathname, biffGetDone(NRRD));
		return -2;				// cannot load file
	}

	m_n = (uint)(hot->axis[0].size);
	m_w = (uint)(hot->axis[1].size);
	m_h = (uint)(hot->axis[2].size);
	m_d = (uint)(hot->axis[3].size);

	if (m_n != 15 && m_n != 28) {
		fprintf(fp, "Not a hot file, only 4th order and 6th order are supported!\n");
		return -3;
	}

	if (m_n == 15)
		m_order = 4;
	else if (m_n == 28)
		m_order = 6;
	else
		;

	m_pHOT = new float[m_w*m_h*m_d*m_n];

	memcpy(m_pHOT, hot->data, sizeof(float)*m_w*m_h*m_d*m_n);
	nrrdNuke(hot);

	fprintf(fp, "Success.\n");
	fprintf(fp, "Width: %d, Height: %d, Depth: %d, Order: %d\n", 
		m_w, m_h, m_d, m_order);
	return 0;
}

void CHOT::GetHOT(const uint x, const uint y, const uint z, float *hot) const
{
	memset(hot, 0, sizeof(float)*m_n);

	if (x < 0 || x > (m_w-1) ||
		y < 0 || y > (m_h-1) ||
		z < 0 || z > (m_d-1)) {
			return;
	}

	uint index = ((z * m_h + y) * m_w + x) * m_n;

	for (uint i = 0; i < m_n; ++i) {
		hot[i] = m_pHOT[index+i];
	}
}

void CHOT::GetHOT(const float x, const float y, const float z, float *hot) const
{
	memset(hot, 0, sizeof(float)*m_n);

	if (x < 0.0f || x > (float)(m_w-1) ||
		y < 0.0f || y > (float)(m_h-1) ||
		z < 0.0f || z > (float)(m_d-1)) {
			return;
	}

	uint x1 = (uint)floor(x);
	uint y1 = (uint)floor(y);
	uint z1 = (uint)floor(z);

	uint x2 = (uint)ceil(x);
	uint y2 = (uint)ceil(y);
	uint z2 = (uint)ceil(z);

	float a = x - (float)x1;
	float b = y - (float)y1;
	float c = z - (float)z1;

	uint i1 = ((z1 * m_h + y1) * m_w + x1)*m_n;
	uint i2 = ((z1 * m_h + y1) * m_w + x2)*m_n;
	uint i3 = ((z1 * m_h + y2) * m_w + x2)*m_n;
	uint i4 = ((z1 * m_h + y2) * m_w + x1)*m_n;
										   
	uint i5 = ((z2 * m_h + y1) * m_w + x1)*m_n;
	uint i6 = ((z2 * m_h + y1) * m_w + x2)*m_n;
	uint i7 = ((z2 * m_h + y2) * m_w + x2)*m_n;
	uint i8 = ((z2 * m_h + y2) * m_w + x1)*m_n;

	for (uint i = 0; i < m_n; ++i) {
		float v1 = m_pHOT[i1+i];
		float v2 = m_pHOT[i2+i];
		float v3 = m_pHOT[i3+i];
		float v4 = m_pHOT[i4+i];
		float v5 = m_pHOT[i5+i];
		float v6 = m_pHOT[i6+i];
		float v7 = m_pHOT[i7+i];
		float v8 = m_pHOT[i8+i];

		float temp = v1 + a * (v2 - v1) + b * (v4 - v1) + c * (v5 - v1) + 
			a * b * (v1 - v2 + v3 - v4) + 
			a * c * (v1 - v2 + v6 - v5) + 
			b * c * (v1 - v4 + v8 - v5) + 
			a * b * c * (-v1 + v2 - v3 + v4 + v5- v6 + v7 - v8);
		hot[i] = temp;
	}
}

void CHOT::GetHOT(const CPoint3F &p, float *hot) const
{
	return GetHOT(p.m_x, p.m_y, p.m_z, hot);
}

void CHOT::GetHOT(const CPoint3F *p, float *hot) const
{
	return GetHOT(p->m_x, p->m_y, p->m_z, hot);
}

void CHOT::GetHOTFast(const uint x, const uint y, const uint z, float *hot) const
{
	uint index = ((z * m_h + y) * m_w + x) * m_n;
	for (uint i = 0; i < m_n; ++i) {
		hot[i] = m_pHOT[index+i];
	}
}

void CHOT::GetHOTFast(const uint index, float *hot) const
{
	for (uint i = 0; i < m_n; ++i) {
		hot[i] = m_pHOT[index*m_n+i];
	}
}

int CHOT::HOT2Directions_One(const float *hot, CPoint3F *dirs) const
{
	const tijk_type *type = NULL;
	if (m_order == 4)
		type = tijk_4o3d_sym;
	else if (m_order == 6)
		type = tijk_6o3d_sym;
	else
		type = NULL;

	float res[28], ten[28];
	tijk_refine_rank1_parm *rank1_parm = tijk_refine_rank1_parm_new();
	rank1_parm->eps_impr = 0.001f;
	tijk_refine_rankk_parm *parm = tijk_refine_rankk_parm_new();
	parm->pos = 1;
	parm->eps_impr = 0.01f;
	parm->rank1_parm = rank1_parm;

	/* first get the positive approximation of the tensor */
	tijk_approx_rankk_3d_f(NULL, NULL, res, hot, type, 6, parm);
	tijk_sub_f(ten, hot, res, type);

	float ls[3];
	float vs[9];
	tijk_approx_rankk_3d_f(ls, vs, res, ten, type, 1, parm);

	dirs[0].m_x = vs[0];
	dirs[0].m_y = vs[1];
	dirs[0].m_z = vs[2];
	return 1;
}

int CHOT::HOT2Directions_Two(const float *hot, CPoint3F *dirs) const
{
	const tijk_type *type = NULL;
	if (m_order == 4)
		type = tijk_4o3d_sym;
	else if (m_order == 6)
		type = tijk_6o3d_sym;
	else
		type = NULL;

	float res[28], ten[28];
	tijk_refine_rank1_parm *rank1_parm = tijk_refine_rank1_parm_new();
	rank1_parm->eps_impr = 0.001f;
	tijk_refine_rankk_parm *parm = tijk_refine_rankk_parm_new();
	parm->pos = 1;
	parm->eps_impr = 0.01f;
	parm->rank1_parm = rank1_parm;

	/* first get the positive approximation of the tensor */
	tijk_approx_rankk_3d_f(NULL, NULL, res, hot, type, 6, parm);
	tijk_sub_f(ten, hot, res, type);

	float ls[3];
	float vs[9];
	tijk_approx_rankk_3d_f(ls, vs, res, ten, type, 2, parm);

	dirs[0].m_x = vs[0];
	dirs[0].m_y = vs[1];
	dirs[0].m_z = vs[2];
	dirs[1].m_x = vs[3];
	dirs[1].m_y = vs[4];
	dirs[1].m_z = vs[5];
	return 2;
}

int CHOT::HOT2Directions(const float *hot, CPoint3F *dirs, float *weights) const
{
	const tijk_type *type = NULL;
	if (m_order == 4)
		type = tijk_4o3d_sym;
	else if (m_order == 6)
		type = tijk_6o3d_sym;
	else
		type = NULL;

	float res[28], ten[28];
	tijk_refine_rank1_parm *rank1_parm = tijk_refine_rank1_parm_new();
	rank1_parm->eps_impr = 0.0001f;
	tijk_refine_rankk_parm *parm = tijk_refine_rankk_parm_new();
	parm->pos = 1;
	parm->eps_impr = 0.0001f;
	parm->rank1_parm = rank1_parm;

	/* first get the positive approximation of the tensor */
	tijk_approx_rankk_3d_f(NULL, NULL, res, hot, type, 6, parm);
	tijk_sub_f(ten, hot, res, type);

	float ls[MAX_DIRECTIONS][3];
	float vs[MAX_DIRECTIONS][9];
	float res_norm[MAX_DIRECTIONS];

	float last_norm = HOTNorm(ten);

	for (int k = 0; k < MAX_DIRECTIONS; ++k) {
		memset(ls[k], 0, sizeof(float)*3);
		memset(vs[k], 0, sizeof(float)*9);
		memset(res, 0, sizeof(float)*m_n);
		tijk_approx_rankk_3d_f(ls[k], vs[k], res, ten, type, (k+1), parm);
		res_norm[k] = HOTNorm(res);
	}

	tijk_refine_rankk_parm_nix(parm);

	/* try rank-1 decomposition */
	if (res_norm[0] < last_norm * HOT_TD_NORM_THRESHOLD) {
		/* success */
		dirs[0].m_x = vs[0][0];
		dirs[0].m_y = vs[0][1];
		dirs[0].m_z = vs[0][2];
		if (weights) {
			weights[0] = ls[0][0];
		}
		return 1;
	}

	/* try rank-2 decomposition */
	if (res_norm[1] < last_norm * HOT_TD_NORM_THRESHOLD) {
		/* success */
		if (ls[1][1] < ls[1][0] * HOT_TD_LS_THRESHOLD) {
			dirs[0].m_x = vs[0][0];
			dirs[0].m_y = vs[0][1];
			dirs[0].m_z = vs[0][2];
			if (weights) {
				weights[0] = ls[1][0];
			}
			return 1;
		} else {
			dirs[0].m_x = vs[1][0];
			dirs[0].m_y = vs[1][1];
			dirs[0].m_z = vs[1][2];
			dirs[1].m_x = vs[1][3];
			dirs[1].m_y = vs[1][4];
			dirs[1].m_z = vs[1][5];
			if (weights) {
				weights[0] = ls[1][0];
				weights[1] = ls[1][1];
			}
			return 2;
		}
	}

	/* try rank-3 decomposition */
	if (ls[2][1] < ls[2][0] * HOT_TD_LS_THRESHOLD) {
		dirs[0].m_x = vs[0][0];
		dirs[0].m_y = vs[0][1];
		dirs[0].m_z = vs[0][2];
		if (weights) {
			weights[0] = ls[2][0];
		}
		return 1;
	} else if (ls[2][2] < ls[2][0] * HOT_TD_LS_THRESHOLD) {
		dirs[0].m_x = vs[1][0];
		dirs[0].m_y = vs[1][1];
		dirs[0].m_z = vs[1][2];
		dirs[1].m_x = vs[1][3];
		dirs[1].m_y = vs[1][4];
		dirs[1].m_z = vs[1][5];
		if (weights) {
			weights[0] = ls[2][0];
			weights[1] = ls[2][1];
		}
		return 2;
	} else {
		dirs[0].m_x = vs[2][0];
		dirs[0].m_y = vs[2][1];
		dirs[0].m_z = vs[2][2];
		dirs[1].m_x = vs[2][3];
		dirs[1].m_y = vs[2][4];
		dirs[1].m_z = vs[2][5];
		dirs[2].m_x = vs[2][6];
		dirs[2].m_y = vs[2][7];
		dirs[2].m_z = vs[2][8];
		if (weights) {
			weights[0] = ls[2][0];
			weights[1] = ls[2][1];
			weights[2] = ls[2][2];
		}
		return 3;
	}
}

float CHOT::ComputeValue(const float *hot, CVector3F *dir) const
{
	float value = 0.0f;
	if (m_order == 4) {
		value += hot[ 0] * factor[0][14] * dir->m_x * dir->m_x * dir->m_x * dir->m_x;
		value += hot[ 1] * factor[0][13] * dir->m_x * dir->m_x * dir->m_x * dir->m_y;
		value += hot[ 2] * factor[0][12] * dir->m_x * dir->m_x * dir->m_x * dir->m_z;
		value += hot[ 3] * factor[0][11] * dir->m_x * dir->m_x * dir->m_y * dir->m_y;
		value += hot[ 4] * factor[0][10] * dir->m_x * dir->m_x * dir->m_y * dir->m_z;
		value += hot[ 5] * factor[0][ 9] * dir->m_x * dir->m_x * dir->m_z * dir->m_z;
		value += hot[ 6] * factor[0][ 8] * dir->m_x * dir->m_y * dir->m_y * dir->m_y;
		value += hot[ 7] * factor[0][ 7] * dir->m_x * dir->m_y * dir->m_y * dir->m_z;
		value += hot[ 8] * factor[0][ 6] * dir->m_x * dir->m_y * dir->m_z * dir->m_z;
		value += hot[ 9] * factor[0][ 5] * dir->m_x * dir->m_z * dir->m_z * dir->m_z;
		value += hot[10] * factor[0][ 4] * dir->m_y * dir->m_y * dir->m_y * dir->m_y;
		value += hot[11] * factor[0][ 3] * dir->m_y * dir->m_y * dir->m_y * dir->m_z;
		value += hot[12] * factor[0][ 2] * dir->m_y * dir->m_y * dir->m_z * dir->m_z;
		value += hot[13] * factor[0][ 1] * dir->m_y * dir->m_z * dir->m_z * dir->m_z;
		value += hot[14] * factor[0][ 0] * dir->m_z * dir->m_z * dir->m_z * dir->m_z;
	} else if (m_order == 6) {
		value += hot[ 0] * factor[1][27] * dir->m_x * dir->m_x * dir->m_x * dir->m_x * dir->m_x * dir->m_x;
		
		value += hot[ 1] * factor[1][26] * dir->m_x * dir->m_x * dir->m_x * dir->m_x * dir->m_x * dir->m_y;
		value += hot[ 2] * factor[1][25] * dir->m_x * dir->m_x * dir->m_x * dir->m_x * dir->m_x * dir->m_z;

		value += hot[ 3] * factor[1][24] * dir->m_x * dir->m_x * dir->m_x * dir->m_x * dir->m_y * dir->m_y;
		value += hot[ 4] * factor[1][23] * dir->m_x * dir->m_x * dir->m_x * dir->m_x * dir->m_y * dir->m_z;
		value += hot[ 5] * factor[1][22] * dir->m_x * dir->m_x * dir->m_x * dir->m_x * dir->m_z * dir->m_z;

		value += hot[ 6] * factor[1][21] * dir->m_x * dir->m_x * dir->m_x * dir->m_y * dir->m_y * dir->m_y;
		value += hot[ 7] * factor[1][20] * dir->m_x * dir->m_x * dir->m_x * dir->m_y * dir->m_y * dir->m_z;
		value += hot[ 8] * factor[1][19] * dir->m_x * dir->m_x * dir->m_x * dir->m_y * dir->m_z * dir->m_z;
		value += hot[ 9] * factor[1][18] * dir->m_x * dir->m_x * dir->m_x * dir->m_z * dir->m_z * dir->m_z;

		value += hot[10] * factor[1][17] * dir->m_x * dir->m_x * dir->m_y * dir->m_y * dir->m_y * dir->m_y;
		value += hot[11] * factor[1][16] * dir->m_x * dir->m_x * dir->m_y * dir->m_y * dir->m_y * dir->m_z;
		value += hot[12] * factor[1][15] * dir->m_x * dir->m_x * dir->m_y * dir->m_y * dir->m_z * dir->m_z;
		value += hot[13] * factor[1][14] * dir->m_x * dir->m_x * dir->m_y * dir->m_z * dir->m_z * dir->m_z;
		value += hot[14] * factor[1][13] * dir->m_x * dir->m_x * dir->m_z * dir->m_z * dir->m_z * dir->m_z;

		value += hot[15] * factor[1][12] * dir->m_x * dir->m_y * dir->m_y * dir->m_y * dir->m_y * dir->m_y;
		value += hot[16] * factor[1][11] * dir->m_x * dir->m_y * dir->m_y * dir->m_y * dir->m_y * dir->m_z;
		value += hot[17] * factor[1][10] * dir->m_x * dir->m_y * dir->m_y * dir->m_y * dir->m_z * dir->m_z;
		value += hot[18] * factor[1][ 9] * dir->m_x * dir->m_y * dir->m_y * dir->m_z * dir->m_z * dir->m_z;
		value += hot[19] * factor[1][ 8] * dir->m_x * dir->m_y * dir->m_z * dir->m_z * dir->m_z * dir->m_z;
		value += hot[20] * factor[1][ 7] * dir->m_x * dir->m_z * dir->m_z * dir->m_z * dir->m_z * dir->m_z;

		value += hot[21] * factor[1][ 6] * dir->m_y * dir->m_y * dir->m_y * dir->m_y * dir->m_y * dir->m_y;
		value += hot[22] * factor[1][ 5] * dir->m_y * dir->m_y * dir->m_y * dir->m_y * dir->m_y * dir->m_z;
		value += hot[23] * factor[1][ 4] * dir->m_y * dir->m_y * dir->m_y * dir->m_y * dir->m_z * dir->m_z;
		value += hot[24] * factor[1][ 3] * dir->m_y * dir->m_y * dir->m_y * dir->m_z * dir->m_z * dir->m_z;
		value += hot[25] * factor[1][ 2] * dir->m_y * dir->m_y * dir->m_z * dir->m_z * dir->m_z * dir->m_z;
		value += hot[26] * factor[1][ 1] * dir->m_y * dir->m_z * dir->m_z * dir->m_z * dir->m_z * dir->m_z;
		value += hot[27] * factor[1][ 0] * dir->m_z * dir->m_z * dir->m_z * dir->m_z * dir->m_z * dir->m_z;
	}

	return value;
}

float CHOT::ComputeValue(const float *hot, CVector3F &dir) const
{
	return this->ComputeValue(hot, &dir);
}

float CHOT::HOTNorm(const float *hot) const
{
	int k = 0;
	if (m_order == 4)
		k = 0;
	else if (m_order == 6)
		k = 1;
	else
		return 0.0f;

	float norm = 0.0f;
	for (uint i = 0; i < m_n; ++i) {
		norm += factor[k][m_n-i-1] * hot[i] * hot[i];
	}
	norm = sqrt(norm);
	return norm;
}
